%**************************************************************************
%BatBot: Biological inspired Bat roBot.

%Copyright RObotics and Cybernetics Group
%Julian Colorado

% Matlab simulator of bat flight behavior. 
%**************************************************************************

%Kinematics: Denavit Hartenberg and basic rotation matrices are used.

%Dynamics: Floating base dynamics equations of motion based on Rigid Body Dynamics (Euler-Lagrange)

%SMA-based Actuation/sensors: Besides flapping, bats perform morphing motion of their wings, by expanding and 
%contracting the wing membrane that improves aerodynamics and maneuverability. Both actuation and sensing
%capacities are addressed using a phenomenological model of SMA Ni? wires working as artificial arrays of muscles. 
%These muscles extend along the sleketon-bat structure of their wings.The phenomenological model is represented by thermo-mechanical
%variable-state equations. SMA are also used as sensors, where Position/Resistance-Resistance/Force relationships are defined.
%**************************************************************************
%clear

%control-points for computing BAT CARPUS and MCP_III trajectories for
%DOwnstroke and Upstroke

%Biological Specimen:
% Carpus = [0.1 0 0.25; 0.18 0 0.22; 0.28 0 0.1; 0.3 0 -0.02; 0.29 0 -0.08; 0.3 0 -0.07; 0.28 0 0.09; 0.2 0 0.2; 0.1 0 0.25];
% MCP_III = [0.35 0 0.2; 0.4 0 0.33; 0.5 0 0.2; 0.55 0 0.02; 0.48 0 -0.25; 0.45 0 -0.28; 0.25 0 -0.18; 0.22 0 -0.05; 0.35 0 0.15]; 
% time = [1 2 3 4 5 6 7 8 9];

%Bat-robot (Simulation)
%Carpus = [5.5 9.5 0; 9.5 6 0;10.5 3.5 0;10.5 1 0;10 4.5 0;10 6 0;7.5 8 0;4.5 10 0]*(0.01/2);

%for transactions: run
%normal (static) - theta3_ref
Carpus = [5.5 9.5 0; 9.5 6 0;10.5 3.5 0;10.5 1 0;10 4.5 0;10 6 0;7.5 8 0;5.5 310 0]*(0.01/2);
% with error at V_air=5m/s
%Carpus = [5.5 9.5 0; 9.5 6 0;10.5 3.5 0;10.5 1 0;10 4.5 0;10 6 0;7.5 8 0;8.5 210 0]*(0.01/2);
% with error at V_air=10m/s
%Carpus = [5.5 9.5 0; 9.5 6 0;10.5 3.5 0;10.5 1 0;10 4.5 0;10 6 0;7.5 8 0;5.5 510 0]*(0.01/2);



MCP_III = [18 9 0; 21.5 9 0;23 4 0;23 -2 0;17 -5.5 0;16 -3.5 0;17.5 1 0;16 5 0]*(0.01/2); 
time = [1 2 3 4 5 6 7 8];

% % %Bat-robot: V3 (ROBOT)
% Carpus = [1.8 6.7 0; 4.8 5 0; 6.8 1.9 0;7 0.5 0; 6.5 3 0; 5.5 4.5 0; 4 6 0; 2.5 7 0]*(0.01);
% MCP_III = [13 6.5 0; 15.5 2.8 0; 17.5 -0.5 0; 17.5 -4 0; 16.8 -1.5 0; 16.3 1.4 0; 15 3.5 0; 13 5.5 0]*(0.01); 
% time = [1 2 3 4 5 6 7 8];

step_time = 0.1;
end_time = 1;

 
%Kinematics model computation for motphing motion
%**************************************************************************
traj_carpus = BatWings_FULLbeat(Carpus,time,step_time,end_time);
traj_MCPIII = BatWings_FULLbeat(MCP_III,time,step_time,end_time);

[f, c] = size(traj_carpus);
%freq = 3.03; %Flapping Frequency (wing-beat cycle)

%freq = 1.15; %(Nominal)Flapping Frequency (wing-beat cycle)
freq = 2.5; %(Overloaded)Flapping Frequency (wing-beat cycle)



flag = 2;  % [2 or 3] Reads y-axis from y or z. Cause motion might be within X-Y or X-Z planes (Bio-trjectories are in Z, then flag=3)


[Q_C, Q_MPCIII, dQ_C, dQ_MPCIII,d2Q_C, d2Q_MPCIII, step] = i_kine_bat_wings(traj_carpus,traj_MCPIII,freq,flag);

%Fixing data
[f, c] = size(Q_C.signals.values(:,1));
[f2, c2] = size(Q_MPCIII.signals.values(:,1));
if f > f2
    f = f2;
else
    f2 = f;
end

%Kinematics model computation of flapping motion (2 DoF at shoulder):
%spherical motion of the shoulder
%**************************************************************************
[Q_flap dQ_flap d2Q_flap] = BatWings_flapping(f-1,freq);



%Parameters for Dynamics simulation
%**************************************************************************
%Parameters used for lagrange model and sliding mode (SM) control 
% l1=0.5; 
% l2=0.5;
% lc1=l1/2;
% lc2=l2/2;
% m1=5; 
% m2=5;
% i1=0.10; 
% i2=0.10;
% g=9.8;

%Parameters used within the inverse dynamics models
%Humerus and radius with digits
l1=0.055;
l2=0.07;
Ltip =.11;
lc1=l1/2;
lc2=l2/2;
m1=0.005; %humerus+SMA muscles
m2=0.003; %radius+digits
i1=(1/3)*m1*l1^2; 
i2=(1/3)*m2*l2^2;
%Shoulder body
m_s=0.006;
r_s=0.02;
i_s=0.5*m_s*r_s^2;

%Parameters for PID SMA actuation+dyn+control
rh = 0.005;  %radio del eslab?n: humerus
Ih = i1;    %humerus inertia
c = 0.007;  %Torsional damping coeficient
mh = 0.008; %total wing: humerus+radius+digits
Xh = lc1;  %distance to link CM
g=9.81;

% %For SMA modeling batbot
% Rsma = 90; %[ohms]
% volt = 10; %[Voltage applied]
% Ro = 1;
% Tau = 0.758;
% Ksm = 1.7;
% %-----

%For SMA modeling iTuna
Rsma = 8; %[ohms]
volt = 4.8; %[Voltage applied]
Ro = 1;
Tau = 0.758;
Ksm = 1.7;
%-----


%Parameters for controller setup
%**************************************************************************
%PID constants
Wn = 4/(Ro*1);
func_d = [1 2*Ro*Wn Wn^2];
poles = roots(func_d);
p_a = abs(poles(1)/2);
p_b = p_a/1.5;
% Kp = (Tau*Ih*((Wn^2)*p_a*p_b))/(Ksm*rh);
% Kd = (Tau*Ih*(p_a*p_b+(2*Ro*Wn*(p_a+p_b))+Wn^2)-(Tau*mh*g*Xh)-c)/(Ksm*rh);
% Ki = (Tau*Ih*((2*Ro*Wn*p_a*p_b)+((p_a+p_b)*Wn^2))-(mh*g*Xh))/(Ksm*rh);
Kp = 70;
Kd = 0.003;
Ki = 0.0115;

%Kinematics Simulations
%**************************************************************************
%Fixing Q for GUI-plotting issues
[fil, col] = size(Q_flap);

%angular joints
Q(:,1) = Q_flap(:,2);  %Shoulder plane motion around gravity axis
Q(:,2) = Q_flap(:,1);  %Shoulder Flapping motion. 


% % FOR biological bat
% Q(:,3) = Q_C.signals.values(1:f,1).*1; %elbow morphing
% Q(:,4) = Q_MPCIII.signals.values(1:f,1).*1; %wrist+digits morphing


% %FOR SIMULATION
% Q(:,3) = Q_C.signals.values(1:f,1).*0.5; %elbow morphing
% Q(:,4) = Q_MPCIII.signals.values(1:f,1).*0.5; %wrist+digits morphing

%% FOR ROBOT
Q(:,3) = Q_C.signals.values(1:f,1).*0.38; %elbow morphing
Q(:,4) = Q_MPCIII.signals.values(1:f,1).*0.38; %wrist+digits morphing


%Joints Velocities
dQ(:,1) = dQ_flap(:,2);
dQ(:,2) = dQ_flap(:,1);
dQ(:,3) = dQ_C.signals.values(1:f,1).*0.1;
dQ(:,4) = dQ_MPCIII.signals.values(1:f,1).*0.1;

%Joints Accelerations
d2Q(:,1) = d2Q_flap(:,2);
d2Q(:,2) = d2Q_flap(:,1);
d2Q(:,3) = d2Q_C.signals.values(1:f,1);
d2Q(:,4) = d2Q_MPCIII.signals.values(1:f,1);

%Testing worrst configuration for flapping force estimation.
%angular joints
% Q(:,1) = zeros(fil,1);
% Q(:,2) = Q_flap(:,1);  %Shoulder Flapping motion. 
% Q(:,3) = zeros(fil,1);
% Q(:,4) = zeros(fil,1);
% %Joints Velocities
% dQ(:,1) = zeros(fil,1);
% dQ(:,2) = dQ_flap(:,1);
% dQ(:,3) = zeros(fil,1);
% dQ(:,4) = zeros(fil,1);
% 
% %Joints Accelerations
% d2Q(:,1) = zeros(fil,1);
% d2Q(:,2) = d2Q_flap(:,1);
% d2Q(:,3) = zeros(fil,1);
% d2Q(:,4) = zeros(fil,1);

F_load = [0 0 0 0 0 -0.001]';
%F_load = [0 0 0 0 0 0]';

%Denavit&Hartenberg parameters
  %      alpha   a    teta        d  rota        m      sx      sy     sz      Ixx      Iyy        Izz    Ixy   Ixz   Iyz fric
L1=link([pi/2    0     Q(1,1)     0    0        m_s     r_s      0     0         0       0          i_s    0     0     0  0],'sta');  
L2=link([-pi/2   l1    Q(1,2)     0    0        m_s     r_s      0     0         0       0          i_s    0     0     0  0],'sta');  
L3=link([0       l2    Q(1,3)     0    0        m1      l1/2     0     0        i1       0          0      0     0     0  0],'sta');  
L4=link([0       Ltip  Q(1,4)     0    0        m2      l2/2     0     0        i2       0          0      0     0     0  0],'sta');  


BAT=robot({L1 L2 L3 L4}); 
BAT.name='BAT';
n=BAT.n;
Q1 = [Q(1,1) Q(1,2) Q(1,3) Q(1,4)];
%Plotting 1 wing-beat cycle for downstroke and upstroke 
%plot(BAT,Q1,'joints');
%drivebot(BAT,Q1);
% 
% %Time Vector

step_2 = (1/freq)/(fil-1);
Ti=0:step_2:(1/freq);
Fext = [0 0 0 0 0 0]';
GRAV = [0 0 0 0 0 -g]';


DHC(1,1:12)= [pi/2     0     Q(1,1)     0    0        m_s         0       0          i_s        r_s      0     0];
DHC(2,1:12)= [-pi/2   l1     Q(1,2)     0    0        m_s         0       0          i_s        r_s      0     0];
DHC(3,1:12)= [0       l2     Q(1,3)     0    0        m1         i1       0          0          l1/2     0     0];
DHC(4,1:12)= [0       Ltip   Q(1,4)     0    0        m2         i2       0          0          l2/2     0     0];

%3D Cartesian Wingtip trajectory Reconstrcution based on Forward kinematics
for i=1:fil
    rot=fkine2(DHC(:,1:5),Q(i,:));
    p(i,:) = rot(1:3,4)'; 
end
%plot3(p(:,1),p(:,2),p(:,3))


%Dynamics Simulations (Peter corke toolbox)
%**************************************************************************
%Computing Forces to generate motion within the Follow-The-Leader assigment
%n: number of robots within the MRS.
%DHC:   alpha  a teta  d  sigma  m  Ixx  Iyy  Izz  sx  sy  sz
%Joint Trajectory:  Q,dQ,d2Q  of nxm, where n=# of trajectory points and m=#degrees of freedom   
%6-dimensional External Force: Fext
%6-dimensional gravity vector: GRAV

%[Torques,K] = inv_dyn_JDC(n,DHC,Q,dQ,d2Q,Fext,GRAV);
 Torques = rne(BAT,Q,dQ,d2Q,[0 0 9.81]',F_load);

   
 
%Joining the signals in order to obtain a complete trajectory profile until T = tf. 
times = 1;
tf = times*(1/freq); 
Time2 = 0:step_2:tf;
lenT = length(Time2);


%Joining torques
len = length(Torques(:,3));
lenT = length(Time2);
cont = 1;
cont2 = 1;

%Joining elbow joint
len_e = length(Q(:,3));
cont_e = 1;

%Joining wrist joint
len_w = length(Q(:,4));
cont_w = 1;


while (cont2 <= times)
    Tau(cont:cont2*len) = Torques(1:len,3);
    cont = cont+len;
    
    Elbow(cont_e:cont2*len_e) = Q(1:len_e,3);
    cont_e = cont_e+len_e;
    
    Wrist(cont_w:cont2*len_w) = Q(1:len_w,4);
    cont_w = cont_w+len_w;
    
    cont2  = cont2+1;
end


%Overloading longer continuos performance up to 5min.
%JUST for showing the decrease in performance for larger periods of time.
% t_total=400s (6.5 min), put "times" variable = 1000. (For normal results, put 
%times = 1)
% 
% zone1 = 0:0.00001:0.15;
% zone2 = 0:0.0000253:0.38;
% cont2 = 1;
% zone3 = 0:0.000011:0.55;
% cont3 = 1;
% 
% Tau = abs(Tau);
% lenF = length(Tau);
% for i=1:lenF
%     if i<=15000
%         Tau(i) = Tau(i)-(Tau(i)*zone1(i));
%     end
%     if i>15000 && i<=30000
%         Tau(i) = (Tau(i)-(Tau(i)*zone2(cont2)))-Tau(i)*0.15;
%         cont2 = cont2+1;
%     end
%     if i>30000
%         Tau(i) = (Tau(i)-(Tau(i)*zone3(cont3)))-Tau(i)*0.53;
%         cont3 = cont3+1;
%     end
% end

%Nominal response for 15 [min] operation
% zone1 = 0:0.000001162:0.05;
% 
% Tau = abs(Tau);
% lenF = length(Tau);
% for i=1:lenF-1  
%         Tau(i) = Tau(i)-(Tau(i)*zone1(i));
% end

%   
% %Structure for torques (Simulink control simulation based on force input
% reference: test.mdl
 T_signals_C = struct('values', [Tau(1:lenT)'], 'dimensions', [1]);
 T_C = struct('time', [Time2], 'signals', T_signals_C);
 
% %Structure for angles (Simulink control simulation based on force input
% reference:  Joint_noise.mdl
 Q_signals2_C = struct('values', [Elbow(1:lenT)'*2.5], 'dimensions', [1]);
 Q_3 = struct('time', [Time2], 'signals', Q_signals2_C);
 
 Q_signals2_MPC = struct('values', [Wrist(1:lenT)'], 'dimensions', [1]);
 Q_4 = struct('time', [Time2], 'signals', Q_signals2_MPC);
  
 
 
 
 
 